"""
src/rotact.py
    Create a Rotational Actuator class derived from the ode HingeJoint.
    This class provides all the hooks necessary to have a fully featured
    joint without a lot of work by the user.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009-2012

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 3 of
the License, or (at your option) any later version. The text of the
GNU Lesser General Public License is included with this library in
the file named LICENSE.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
"""

from pandac.PandaModules import OdeHingeJoint
Infinity = 1e10000
NaN = Infinity / Infinity

class RotationalActuator(OdeHingeJoint):
    """ Can behave like a spring hinge, rotational damper, motor, or
    a combination of all 3."""
    # Provide new so we can have subclasses that add parameters
    def __new__(cls, *args, **kwargs):
        # Initialize the HingeJoint class
        return OdeHingeJoint.__new__(cls, args[0]) # First arg is World

    def __init__(self, World):
        # Initialize the HingeJoint class
        OdeHingeJoint.__init__(self, World)

        # Initialize the properties to benign values
        self._k = 0.0
        self._k0 = 0.0
        self._lockedTorqueLimit = 1.0e99
        self._staticFrictionTorque = 0.0
        self._kineticFrictionTorque = 0.0 * self._staticFrictionTorque
        self._frictionTol = 0.0001
        self._loStop = -Infinity
        self._hiStop = Infinity

        self._externalTorque = 0.0

        self._isLocked = False

        # Set the angular limits
        self.setParamLoStop(self._loStop)
        self.setParamHiStop(self._hiStop)

        # Make the stops non bouncy
        self.setParamBounce(0.0)

        # Set the target motor rate to 0 radians/sec
        self.setParamVel(0.0)

        # Set the motor to the static friction torque
        self.setParamFMax(self._staticFrictionTorque)

    def GetProperties(self):
        properties = (self._k, self._lockedTorqueLimit, self._staticFrictionTorque,
                        self._kineticFrictionTorque, self._frictionTol)

        return properties

    def ChangeProperties(self, SpringConstant=None, SpringNullAngle=None, LockedTorqueLimit=None,
                            StaticFrictionTorque=None, KineticFrictionTorque=None,
                            FrictionTol=None, LoStop=None, HiStop=None):
        # Make changes to specified properties
        if SpringConstant is not None:
            self._k = SpringConstant
        if SpringNullAngle is not None:
            self._k0 = SpringNullAngle
        if LockedTorqueLimit is not None:
            self._lockedTorqueLimit = LockedTorqueLimit
        if StaticFrictionTorque is not None:
            self._staticFrictionTorque = StaticFrictionTorque
        if KineticFrictionTorque is not None:
            self._kineticFrictionTorque = KineticFrictionTorque
        if FrictionTol is not None:
            self._frictionTol = FrictionTol
        if LoStop is not None:
            self._loStop = LoStop
        if HiStop is not None:
            self._hiStop = HiStop

    def GetExternalTorque(self):
        """Get the external torque currently being applied to the hinge joint."""
        return self._externalTorque

    def SetExternalTorque(self, Torque):
        """Set the external torque to apply to the hinge joint.  This is applied
        along with hinge and frictional torques every time UpdateSettings() is called."""
        self._externalTorque = Torque

    def UpdateSettings(self):
        """This function must be called before each time step is executed.  It computes
        and updates the torque applied to the joint.  It also handles joint locking."""
        #print self._Locked
        #print self.GetProperties()

        # Set the angular limits
        self.setParamLoStop(self._loStop)
        self.setParamHiStop(self._hiStop)

        if self._isLocked:
            self.setParamFMax(self._lockedTorqueLimit)
        else:
            # Set the motor to the static friction torque
            self.setParamFMax(self._staticFrictionTorque)

        # Compute spring torque
        angle = self.getAngle()
        #print 'Angle = ', angle
        springTorque = self._k * (self._k0 - angle)

        # Compute kinetic friction torque
        angularVelocity = self.getAngleRate()
        if angularVelocity < -self._frictionTol:
            kineticFrictionTorque = self._kineticFrictionTorque
        elif angularVelocity > self._frictionTol:
            kineticFrictionTorque = -self._kineticFrictionTorque
        else:
            kineticFrictionTorque = 0.0

        sumTorques = springTorque + kineticFrictionTorque + self._externalTorque
        #print sumTorques, SpringTorque, KineticFrictionTorque, self._ExternalTorque

        self.addTorque(sumTorques)

    def Lock(self):
        """Lock the joint so it will not rotate."""
        self._isLocked = True

    def Unlock(self):
        """Unlock the joint so it will rotate freely."""
        self._isLocked = False

